#include "peripherals.h"

/* ---------------------------------------------------------------------------*
 * LED methods                                                                *
 * ---------------------------------------------------------------------------*/

void setGreenOff() {
    PORTAbits.RA0 = 0;
}
void setGreenOn() {
    PORTAbits.RA0 = 1;
}
void setRedOff() {
    PORTAbits.RA1 = 0;
}
void setRedOn() {
    PORTAbits.RA1 = 1;
}
void setBlueOff() {
    PORTAbits.RA2 = 0;
}
void setBlueOn() {
    PORTAbits.RA2 = 1;
}
void setLowBattOff() {
    PORTAbits.RA3 = 0;
}
void setLowBattOn() {
    PORTAbits.RA3 = 1;
}

/* ---------------------------------------------------------------------------*
 * Servo methods                                                              *
 * ---------------------------------------------------------------------------*/

// Base right servo method.  All other right servo operations call this.
// Call with data == READ_SPEED to READ motor (will not set speed)
// Always returns current right speed.
unsigned char rightServoRun(unsigned char data) {
    static unsigned char speed = 0;
    if (data != READ_SPEED) {
        CCP1CON=0x0C | ((data & 0x03)<<4);
        CCPR1L=data>>2;
        speed = data;
    }

    return speed;
}

unsigned char rightServoRead() {
    return rightServoRun(READ_SPEED);
}

void rightServoRunOffset(unsigned char speedOffset) {
    unsigned char rightSpeed = RIGHT_SERVO_NEUTRAL + speedOffset;
    if (rightSpeed > RIGHT_SERVO_GO_MAX) {
        rightSpeed = RIGHT_SERVO_GO_MAX;
    }
    rightServoRun(rightSpeed);
}

void rightServoStop() {
    rightServoRun(RIGHT_SERVO_STOP);
}

void rightServoGo() {
    rightServoRun(RIGHT_SERVO_GO_MAX);
}

void rightServoReverse() {
    rightServoRun(RIGHT_SERVO_REVERSE_MAX);
}

void rightServoGoVar() {
    unsigned char speedOffset = SPI();
    rightServoRunOffset(speedOffset);
}

void rightServoReverseVar() {
    unsigned char speedOffset = SPI();
    rightServoRunOffset(-speedOffset);
}

// Base left servo method.  All other right servo operations call this.
// Call with data == READ_SPEED to READ motor (will not set speed)
// Always returns current left speed.
unsigned char leftServoRun(unsigned char data) {
    static unsigned char speed = 0;
    if (data != READ_SPEED) {
        CCP2CON=0x0C | ((data & 0x03)<<4);
        CCPR2L=data>>2;
        speed = data;
    }

    return speed;
}

unsigned char leftServoRead() {
    return leftServoRun(READ_SPEED);
}

void leftServoRunOffset(unsigned char speedOffset) {
    unsigned char leftSpeed = LEFT_SERVO_NEUTRAL - speedOffset;
    if (leftSpeed < LEFT_SERVO_GO_MAX) {
        leftSpeed = LEFT_SERVO_GO_MAX;
    }
    leftServoRun(leftSpeed);
}

void leftServoStop() {
    leftServoRun(LEFT_SERVO_STOP);
}

void leftServoGo() {
    leftServoRun(LEFT_SERVO_GO_MAX);
}

void leftServoReverse() {
    leftServoRun(LEFT_SERVO_REVERSE_MAX);
}

void leftServoGoVar() {
    unsigned char speedOffset = SPI();
    leftServoRunOffset(speedOffset);
}

void leftServoReverseVar() {
    unsigned char speedOffset = SPI();
    leftServoRunOffset(-speedOffset);
}

void bothServosStop() {
    rightServoStop();
    leftServoStop();
}

void bothServosGo() {
    rightServoRun(RIGHT_SERVO_GO_MAX);
    leftServoRun(LEFT_SERVO_GO_MAX);
}

void bothServosReverse() {
    rightServoRun(RIGHT_SERVO_REVERSE_MAX);
    leftServoRun(LEFT_SERVO_REVERSE_MAX);
}

void bothServosGoVar() {
    unsigned char speedOffset = SPI();
    rightServoRunOffset(speedOffset);
    leftServoRunOffset(speedOffset);
}

void bothServosReverseVar() {
    unsigned char speedOffset = SPI();
    rightServoRunOffset(-speedOffset);
    leftServoRunOffset(-speedOffset);
}

void spinRight() {
    rightServoReverse();
    leftServoGo();
}

void spinLeft() {
    rightServoGo();
    leftServoReverse();
}

void turnRight() {
    rightServoStop();
    leftServoGo();
}

void turnLeft() {
    rightServoGo();
    leftServoStop();
}

void spinRightVar() {
    unsigned char speedOffset = SPI();
    rightServoRunOffset(-speedOffset);
    leftServoRunOffset(speedOffset);
}

void spinLeftVar() {
    unsigned char speedOffset = SPI();
    rightServoRunOffset(speedOffset);
    leftServoRunOffset(-speedOffset);
}

void turnRightVar() {
    rightServoStop();
    leftServoRunOffset(SPI());
}

void turnLeftVar() {
    rightServoRunOffset(SPI());
    leftServoStop();
}

// Debug methods



/* ---------------------------------------------------------------------------*
 * Bumper methods                                                             *
 * ---------------------------------------------------------------------------*/

unsigned char getBumpers() {
    return (PORTBbits.RB5 << 1) | PORTBbits.RB4;
}

/* ---------------------------------------------------------------------------*
 * SPI methods                                                             *
 * ---------------------------------------------------------------------------*/

unsigned char SPI() {
    unsigned char retData;

    SSP2STATbits.BF = 0;
    while(SSP2STATbits.BF == 0);
    retData = SSP2BUF;
    while(SSP2STATbits.BF == 1);
    SSP2BUF = getBumpers();
    return retData;
}